//
// Created by zss on 18-4-18.
//
#include "MapPoint.h"
#include "ORBmatcher.h"
#include "Object.h"
namespace myslam
{
    long unsigned int MapPoint::nNextId=0;
    mutex MapPoint::mGlobalMutex;


    //todo 用关键帧创建地图点
    MapPoint::MapPoint(const cv::Mat &Pos, KeyFrame *pRefKF, Map* pMap):
            mnFirstKFid(pRefKF->mnId), mnFirstFrame(pRefKF->mnFrameId), nObs(0), mnTrackReferenceForFrame(0),
            mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0),
            mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(pRefKF), mnVisible(1), mnFound(1), mbBad(false),
            mpReplaced(static_cast<MapPoint*>(NULL)), mfMinDistance(0), mfMaxDistance(0), mpMap(pMap),object_id(-1),addid(-1),have_feature(false)
    {
        Pos.copyTo(mWorldPos);
        mNormalVector = cv::Mat::zeros(3,1,CV_32F);

        // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
        unique_lock<mutex> lock(mpMap->mMutexPointCreation);
        mnId=nNextId++;
    }

    MapPoint::MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF):
            mnFirstKFid(-1), mnFirstFrame(pFrame->mnId), nObs(0), mnTrackReferenceForFrame(0), mnLastFrameSeen(0),
            mnBALocalForKF(0), mnFuseCandidateForKF(0),mnLoopPointForKF(0), mnCorrectedByKF(0),
            mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(static_cast<KeyFrame*>(NULL)), mnVisible(1),
            mnFound(1), mbBad(false), mpReplaced(NULL), mpMap(pMap),object_id(-1),addid(-1),have_feature(false)
    {
        Pos.copyTo(mWorldPos);
        cv::Mat Ow = pFrame->GetCameraCenter();
        mNormalVector = mWorldPos - Ow;
        mNormalVector = mNormalVector/cv::norm(mNormalVector);

        cv::Mat PC = Pos - Ow;
        const float dist = cv::norm(PC);
        const int level = pFrame->mvKeysUn[idxF].octave;
        const float levelScaleFactor =  pFrame->mvScaleFactors[level];
        const int nLevels = pFrame->mnScaleLevels;

        mfMaxDistance = dist*levelScaleFactor;
        mfMinDistance = mfMaxDistance/pFrame->mvScaleFactors[nLevels-1];

        pFrame->mDescriptors.row(idxF).copyTo(mDescriptor);

        // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
        unique_lock<mutex> lock(mpMap->mMutexPointCreation);
        mnId=nNextId++;
    }

    void MapPoint::SetWorldPos(const cv::Mat &Pos)
    {
        unique_lock<mutex> lock2(mGlobalMutex);
        unique_lock<mutex> lock(mMutexPos);
        Pos.copyTo(mWorldPos);
    }

    cv::Mat MapPoint::GetWorldPos()
    {
        unique_lock<mutex> lock(mMutexPos);
        return mWorldPos.clone();
    }

    cv::Mat MapPoint::GetNormal()
    {
        unique_lock<mutex> lock(mMutexPos);
        return mNormalVector.clone();
    }

    KeyFrame* MapPoint::GetReferenceKeyFrame()
    {
        unique_lock<mutex> lock(mMutexFeatures);
        return mpRefKF;
    }

    void MapPoint::AddObservation(KeyFrame* pKF, size_t idx)
    {
        unique_lock<mutex> lock(mMutexFeatures);
        if(mObservations.count(pKF))
            return;
        mObservations[pKF]=idx;

        if(pKF->mvuRight[idx]>=0)
        {
            nObs+=2;
        }

        else
            nObs++;
    }

    void MapPoint::EraseObservation(KeyFrame* pKF)
    {
        bool bBad=false;
        {
            unique_lock<mutex> lock(mMutexFeatures);
            if(mObservations.count(pKF))
            {
                int idx = mObservations[pKF];
                if(pKF->mvuRight[idx]>=0)
                {
                    nObs-=2;
                }

                else
                {
                    nObs--;
                }



                mObservations.erase(pKF);

                if(mpRefKF==pKF)
                    mpRefKF=mObservations.begin()->first;

                // todo nObs为了这里 If only 2 observations or less, discard point
                if(nObs<=2)
                    bBad=true;
            }
        }

        if(bBad)
        {
            SetBadFlag();
        }



    }

    map<KeyFrame*, size_t> MapPoint::GetObservations()
    {
        unique_lock<mutex> lock(mMutexFeatures);
        return mObservations;
    }

    int MapPoint::Observations()
    {
        unique_lock<mutex> lock(mMutexFeatures);
        return nObs;
    }

    void MapPoint::SetBadFlag()
    {
        map<KeyFrame*,size_t> obs;
        {
            unique_lock<mutex> lock1(mMutexFeatures);
            unique_lock<mutex> lock2(mMutexPos);
            mbBad=true;
            obs = mObservations;
            mObservations.clear();
        }
        for(map<KeyFrame*,size_t>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
        {
            KeyFrame* pKF = mit->first;
            pKF->EraseMapPointMatch(mit->second);
        }

        mpMap->EraseMapPoint(this);
    }

    MapPoint* MapPoint::GetReplaced()
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        return mpReplaced;
    }

    void MapPoint::Replace(MapPoint* pMP)
    {
        //cout<<"---------Replace---------"<<endl;
        if(pMP->mnId==this->mnId)
            return;

        int nvisible, nfound;
        map<KeyFrame*,size_t> obs;
        {
            unique_lock<mutex> lock1(mMutexFeatures);
            unique_lock<mutex> lock2(mMutexPos);
            obs=mObservations;
            mObservations.clear();
            //cout<<"-----------------------------------------------"<<endl;
            mbBad=true;
            nvisible = mnVisible;
            nfound = mnFound;
            mpReplaced = pMP;
        }

        for(map<KeyFrame*,size_t>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
        {
            // Replace measurement in keyframe
            KeyFrame* pKF = mit->first;

            if(!pMP->IsInKeyFrame(pKF))
            {
                pKF->ReplaceMapPointMatch(mit->second, pMP);//todo mvpMapPoints[mit->second]=pMP
                pMP->AddObservation(pKF,mit->second);//todo mObservations[pKF]=idx;
            }
            else
            {
                //todo 剔除MapPoint不仅需要删掉地图点中维护的关键帧观测关系，还需要删掉对应关键中对应的地图点，以及Map中该地图点的内存：
                //todo 如果新建立的点也在这个帧观测到了，就直接把重复的删掉就好了，保证只剩下新建立的那个点
                pKF->EraseMapPointMatch(mit->second);
            }
        }

        pMP->IncreaseFound(nfound);
        pMP->IncreaseVisible(nvisible);
        pMP->ComputeDistinctiveDescriptors();
//        for (int i = 0; i < mpMap->objs_real.size() ; ++i)
//        {
//            if (i == object_id)
//            {
//                for (int j = 0; j < mpMap->objs_real[i]->MapPonits.size(); ++j)
//
//                {
//                    if (mpMap->objs_real[i]->MapPonits[j]->isBad())
//                        mpMap->objs_real[i]->MapPonits.erase(mpMap->objs_real[i]->MapPonits.begin() + j);
//                }
//                cv::Mat x3D=pMP->GetWorldPos();
//                float depth=x3D.at<float>(2,0);
//                float middle_point=mpMap->objs_real[i]->_Pos.at<float>(2,0);
//                if(abs(depth-middle_point)<0.2)
//                {
//                    mpMap->objs_real[i]->MapPonits.push_back(pMP);
//                }
//
//            }
//        }
        mpMap->EraseMapPoint(this);

    }

    bool MapPoint::isBad()
    {
        unique_lock<mutex> lock(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        return mbBad;
    }

    void MapPoint::IncreaseVisible(int n)
    {
        unique_lock<mutex> lock(mMutexFeatures);
        mnVisible+=n;
    }

    void MapPoint::IncreaseFound(int n)
    {
        unique_lock<mutex> lock(mMutexFeatures);
        mnFound+=n;
    }

    float MapPoint::GetFoundRatio()
    {
        unique_lock<mutex> lock(mMutexFeatures);
        return static_cast<float>(mnFound)/mnVisible;
    }

    void MapPoint::ComputeDistinctiveDescriptors()
    {
        // Retrieve all observed descriptors
        vector<cv::Mat> vDescriptors;

        map<KeyFrame*,size_t> observations;

        {
            unique_lock<mutex> lock1(mMutexFeatures);
            if(mbBad)
                return;
            observations=mObservations;
        }

        if(observations.empty())
            return;

        vDescriptors.reserve(observations.size());

        for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
        {
            KeyFrame* pKF = mit->first;

            if(!pKF->isBad())
                vDescriptors.push_back(pKF->mDescriptors.row(mit->second));//todo 观测关键帧中特征点对应的描述子
        }

        if(vDescriptors.empty())
            return;

        // Compute distances between them
        const size_t N = vDescriptors.size();

        float Distances[N][N];
        for(size_t i=0;i<N;i++)
        {
            Distances[i][i]=0;
            for(size_t j=i+1;j<N;j++)
            {
                int distij = ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]);
                Distances[i][j]=distij;
                Distances[j][i]=distij;
            }
        }

        // Take the descriptor with least median distance to the rest//todo 计算所有观测的对应特征点最好的描述子给这个地图点
        int BestMedian = INT_MAX;
        int BestIdx = 0;
        for(size_t i=0;i<N;i++)
        {
            vector<int> vDists(Distances[i],Distances[i]+N);
            sort(vDists.begin(),vDists.end());
            int median = vDists[0.5*(N-1)];

            if(median<BestMedian)
            {
                BestMedian = median;
                BestIdx = i;
            }
        }

        {
            unique_lock<mutex> lock(mMutexFeatures);
            mDescriptor = vDescriptors[BestIdx].clone();
        }
    }

    cv::Mat MapPoint::GetDescriptor()
    {
        unique_lock<mutex> lock(mMutexFeatures);
        return mDescriptor.clone();
    }

    int MapPoint::GetIndexInKeyFrame(KeyFrame *pKF)
    {
        unique_lock<mutex> lock(mMutexFeatures);
        if(mObservations.count(pKF))
            return mObservations[pKF];
        else
            return -1;
    }

    bool MapPoint::IsInKeyFrame(KeyFrame *pKF)
    {
        unique_lock<mutex> lock(mMutexFeatures);
        return (mObservations.count(pKF));
    }

    void MapPoint::UpdateNormalAndDepth()
    {
        map<KeyFrame*,size_t> observations;
        KeyFrame* pRefKF;
        cv::Mat Pos;
        {
            unique_lock<mutex> lock1(mMutexFeatures);
            unique_lock<mutex> lock2(mMutexPos);
            if(mbBad)
                return;
            observations=mObservations;
            pRefKF=mpRefKF;
            Pos = mWorldPos.clone();
        }

        if(observations.empty())
            return;

        cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
        int n=0;
        for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
        {
            KeyFrame* pKF = mit->first;
            cv::Mat Owi = pKF->GetCameraCenter();
            cv::Mat normali = mWorldPos - Owi;
            normal = normal + normali/cv::norm(normali);
            n++;
        }

        cv::Mat PC = Pos - pRefKF->GetCameraCenter();
        const float dist = cv::norm(PC);
        const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;
        const float levelScaleFactor =  pRefKF->mvScaleFactors[level];
        const int nLevels = pRefKF->mnScaleLevels;

        {
            unique_lock<mutex> lock3(mMutexPos);
            mfMaxDistance = dist*levelScaleFactor;
            mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];
            mNormalVector = normal/n;
        }
    }

    float MapPoint::GetMinDistanceInvariance()
    {
        unique_lock<mutex> lock(mMutexPos);
        return 0.8f*mfMinDistance;
    }

    float MapPoint::GetMaxDistanceInvariance()
    {
        unique_lock<mutex> lock(mMutexPos);
        return 1.2f*mfMaxDistance;
    }

    int MapPoint::PredictScale(const float &currentDist, KeyFrame* pKF)
    {
        float ratio;
        {
            unique_lock<mutex> lock(mMutexPos);
            ratio = mfMaxDistance/currentDist;
        }

        int nScale = ceil(log(ratio)/pKF->mfLogScaleFactor);
        if(nScale<0)
            nScale = 0;
        else if(nScale>=pKF->mnScaleLevels)
            nScale = pKF->mnScaleLevels-1;

        return nScale;
    }

    int MapPoint::PredictScale(const float &currentDist, Frame* pF)
    {
        float ratio;
        {
            unique_lock<mutex> lock(mMutexPos);
            ratio = mfMaxDistance/currentDist;
        }

        int nScale = ceil(log(ratio)/pF->mfLogScaleFactor);
        if(nScale<0)
            nScale = 0;
        else if(nScale>=pF->mnScaleLevels)
            nScale = pF->mnScaleLevels-1;

        return nScale;
    }
}

